/* --------------------------------------------------------------------------
 * This code is written by Przemek Jaworski, Flora Salim, Rafael Urquiza, 
 * Rafael Roa, and Jose Luis in SmartGeometry 2011: Interacting with the City
 * based on the sample code from SimpleOpenNI: DepthMap3d Test
 * --------------------------------------------------------------------------
 * SimpleOpenNI is Processing Wrapper for the OpenNI/Kinect library
 * http://code.google.com/p/simple-openni
 * ----------------------------------------------------------------------------
 */
// import UDP library 
import hypermedia.net.*; 

String message = ""; // the message to send 
String ip = "127.0.0.1"; // the IP address where UbiMash is hosted on 
int port = 10003; // the destination port where UbiMash is listening on 7778888877778
UDP udps; // define the UDP object


float zCutOff = 980;
float shiftX = -260;
float shiftY = 60;

import SimpleOpenNI.*;

SimpleOpenNI context;
float        zoomF =.8f;
float        rotX = radians(180);  // by default rotate the hole scene 180deg around the x-axis, 
                                   // the data from openni comes upside down
float        rotY = radians(0);

void setup()
{
   frameRate(300);
  
  size(1280,800,P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem

  //context = new SimpleOpenNI(this,SimpleOpenNI.RUN_MODE_SINGLE_THREADED);
  context = new SimpleOpenNI(this);

  // disable mirror
  context.setMirror(true);

  // enable depthMap generation 
  context.enableDepth();

  stroke(255,255,255);
  smooth();
  perspective(95,float(width)/float(height), 
              10,150000);
 
  // create a new datagram connection
  // and wait for incomming message
  udps = new UDP( ip, port );
  //udp.log( true ); 		// <-- printout the connection activit
  publishVar("ptList","TEST"); 
  String testStr = "{-187:-427:892;-484:-357:944;-432:-359:940;-383:-359:940;-336:-357:944;-182:-380:891;-483:-309:942;-433:-309:941;-385:-309:943;-336:-308:945;-202:-332:896;-482:-261:941;-434:-259:943;-386:-258:947;-337:-258:946;-185:-267:929;-482:-212:941;-435:-209:945;-386:-208:947;-337:-208:946;-477:-168:932;-384:-164:939;-335:-162:941;-285:-158:947;322:-153:956;368:-161:944;411:-169:931;452:-179:915;-360:-280:702;-324:-279:703;36:-137:907;80:-161:872;124:-171:859;173:-156:879;226:-135:910;278:-125:924;345:-104:954;391:-113:941;432:-126:924;32:-83:916;76:-113:876;119:-128:855;165:-122:864;215:-108:882;271:-83:916;328:-63:943;370:-76:925;410:-90:907;449:-102:891;-201:-85:854;32:-33:919;77:-63:882;121:-76:866;168:-68:875;218:-53:893;373:-12:946;409:-36:915;442:-59:886;-232:-28:869;37:14:918;83:1:903;127:-19:880;174:-14:885;227:9:912;379:39:948;416:17:921;447:-13:886;46:58:914;93:49:904;137:32:885;185:40:894;241:73:930;374:88:947;415:71:928;446:41:895;94:109:917}";
  int len = testStr.length();
  println("strlen:" + len);
  publishVar("ptList",testStr);
}

void publishVar(String name, String variableValue) 
{ 
  message = "Publish,"; 
  message += name+","+variableValue;  // nf(variableValue,0,0); 
// send the message 
  udps.send( message, ip, port );
   println(message); 
}

void draw()
{
  // update the cam
  context.update();
  background(0,0,0);

  translate(width/2, height/2, 0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoomF);

  int[]   depthMap = context.depthMap();
  int     steps   = 3;  // to speed up the drawing, draw every third point
  int     index;
  
  translate(shiftX,shiftY,-1000);  // set the rotation center of the scene 1000 infront of the camera

  stroke(255);
  
  numCPoints = 0;
  
  //=================================================================
  //main loop - draw points in 3d
  PVector[] realWorldMap = context.depthMapRealWorld();
  for(int y=0;y < context.depthHeight();y+=steps)
  {
    
    oldPoint = new PVector(0,0,0);
    numPointsBuffered = 0;
  
    for(int x=0;  x < context.depthWidth();x+=steps)
    {
      index = x + y * context.depthWidth();
      if(depthMap[index] > 0)
      { 
        realWorldPoint = realWorldMap[index];
        realWorldPoint.y=-realWorldPoint.y;//flip y axis
        realWorldPoint.y += (realWorldPoint.z-zCutOff)*0.8;
        
        if ((realWorldPoint.z < zCutOff))
        {
            {
              if ((!(oldPoint.z==0))&&((y%30)==0)&&(dist(oldPoint.x,oldPoint.y,oldPoint.z, realWorldPoint.x,realWorldPoint.y,realWorldPoint.z)<20))
              {
                 stroke(255,0,0);
                 strokeWeight(5);
                  //draw line
                 strokeWeight(1);
                 stroke(255);
                 
                 //catch control points
                   pointBuffer[numPointsBuffered] = new PVector(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
                   numPointsBuffered++;
                   
                   if (numPointsBuffered>=10)
                   {
                     
                     float xx=0;
                     float yy=0;
                     float zz=0;
                      for (int j=0;j<10;j++)
                       {
                           xx+=pointBuffer[j].x*0.1;
                           yy+=pointBuffer[j].y*0.1;
                           zz+=pointBuffer[j].z*0.1;
                       }   
                     //add new control point
                     cPoints[numCPoints] = new controlPoint();
                     cPoints[numCPoints].x = xx;
                     cPoints[numCPoints].y = yy;
                     cPoints[numCPoints].z = zz;
                     numCPoints++;
                     
                     numPointsBuffered=0; //reset the buffer
                   }
                 //--------------------
              }
              else
              {
                numPointsBuffered=0;
              }
              
              //draw point!  
              point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);  // make realworld z negative, in the 3d drawing coordsystem +z points in the direction of the eye
            
          
            }
            //else 
            oldPoint = realWorldPoint;            
        }  
      }
    }
  } 
  //=================================================================
  //main loop - finished drawing points in 3d
  
  drawControlPoints();

   println("zcutoff = "+ zCutOff + " zoom = "+ zoomF+ " shiftX = "+ shiftX+ " shiftY = "+ shiftY);
  // draw the kinect cam
  context.drawCamFrustum();
}


void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  
  case '1':
    zCutOff-=10;
    break;
  case '2':
    zCutOff+=10;
    break;
  case '3':
    zoomF-=0.05;
    break;
  case '4':
    zoomF+=0.05;
    break;
  case '5':
    shiftX-=10;
    break;
  case '6':
    shiftX+=10;
    break;
  case '7':
    shiftY-=10;
    break;
  case '8':
    shiftY+=10;
    break;
  }

  switch(keyCode)
  {
  case LEFT:
    rotY += 0.1f;
    break;
  case RIGHT:
    // zoom out
    rotY -= 0.1f;
    break;
  case UP:
    if(keyEvent.isShiftDown())
      zoomF += 0.02f;
    else
      rotX += 0.1f;
    break;
  case DOWN:
    if(keyEvent.isShiftDown())
    {
      zoomF -= 0.02f;
      if(zoomF < 0.01)
        zoomF = 0.01;
    }
    else
      rotX -= 0.1f;
    break;
  }
}

